//
// Created by 35702 on 2025/2/26.
//


#include "stm32h7xx_hal.h"
#include "tim.h"
#include "stm32h7xx_hal_tim.h"
#include "main.h"
#include "Car_DIR.h"
#include "str.h"
#include "math.h"

#define accel        200
#define decel        200
#define max_speed    100




/*向上走*/
void  motor_Go(char flag ,int data)
{
data=data/1.8*16;  /*16细分*/



    switch (flag) // 状态机函数
    {
/*-------------------------------------------------------------------前 进------------------------------------------------------------------*/
        case DIR_UP:
            HAL_GPIO_WritePin(GPIOC, DIR1_Pin, 0);
            HAL_GPIO_WritePin(GPIOC, DIR2_Pin, 1);
            HAL_GPIO_WritePin(GPIOB, DIR4_Pin, 0);
            HAL_GPIO_WritePin(GPIOB, DIR3_Pin, 1);
            break;

/*-------------------------------------------------------------------后 退------------------------------------------------------------------*/
        case DIR_Down:
            HAL_GPIO_WritePin(GPIOC, DIR1_Pin, 1);
            HAL_GPIO_WritePin(GPIOC, DIR2_Pin, 0);
            HAL_GPIO_WritePin(GPIOB, DIR4_Pin, 1);
            HAL_GPIO_WritePin(GPIOB, DIR3_Pin, 0);
            break;
/*-------------------------------------------------------------------左平移 ------------------------------------------------------------------*/
        case DIR_Left:
            HAL_GPIO_WritePin(GPIOC, DIR1_Pin, 1);
            HAL_GPIO_WritePin(GPIOC, DIR2_Pin, 1);
            HAL_GPIO_WritePin(GPIOB, DIR4_Pin, 0);
            HAL_GPIO_WritePin(GPIOB, DIR3_Pin, 1);
            break;
/*-------------------------------------------------------------------右平移 ------------------------------------------------------------------*/
        case DIR_Right:
            HAL_GPIO_WritePin(GPIOC, DIR1_Pin, 0);
            HAL_GPIO_WritePin(GPIOC, DIR2_Pin, 0);
            HAL_GPIO_WritePin(GPIOB, DIR4_Pin, 1);
            HAL_GPIO_WritePin(GPIOB, DIR3_Pin, 0);
            break;
/*-------------------------------------------------------------------左转------------------------------------------------------------------*/
        case Left_rotate:
            HAL_GPIO_WritePin(GPIOC, DIR1_Pin, 0);
            HAL_GPIO_WritePin(GPIOC, DIR2_Pin, 0);
            HAL_GPIO_WritePin(GPIOB, DIR4_Pin, 1);
            HAL_GPIO_WritePin(GPIOB, DIR3_Pin, 0);
            break;
/*-------------------------------------------------------------------右转 ------------------------------------------------------------------*/
        case Right_rotate:

            HAL_GPIO_WritePin(GPIOC, DIR1_Pin, 0);
            HAL_GPIO_WritePin(GPIOC, DIR2_Pin, 0);
            HAL_GPIO_WritePin(GPIOB, DIR4_Pin, 1);
            HAL_GPIO_WritePin(GPIOB, DIR3_Pin, 0);


            break;


    }

    Get_Motor_1(0, abs(data), 300, 500, 100,&htim1,TIM_CHANNEL_1);
    Get_Motor_1(1, abs(angle), 300, 500, 100,&htim1,TIM_CHANNEL_2);
    Get_Motor_1(2, abs(angle), 300, 500, 100,&htim1,TIM_CHANNEL_3);
    Get_Motor_1(3, abs(angle), 300, 500, 100,&htim1,TIM_CHANNEL_3);



}




void  PTZ(int  angle)
{
angle =angle/1.8 *32;  /*32细分*/

             if(angle<0)
             {HAL_GPIO_WritePin(GPIOE, DIR5_Pin,1); }
             else
             {HAL_GPIO_WritePin(GPIOE, DIR5_Pin,0); }

Get_Motor_1(4, abs(angle), 300, 500, 100,&htim8,TIM_CHANNEL_3);
}


//  机械臂
void  mechanical_arm(int  angle)
{
    angle =angle/1.8 *32;
    if(angle<0)
    {
        HAL_GPIO_WritePin(GPIOB, DIR6_Pin,0);
    }
    else
    {
        HAL_GPIO_WritePin(GPIOB, DIR6_Pin,1);
    }
    Get_Motor_1(5, abs(angle), 300, 500, 30,&htim8,TIM_CHANNEL_4);
}








